#include "WPILib.h"
#include <Timer.h> //Make sure to include this in the final code. 
//This will be used to count time for the solenoid.
#include <DigitalInput.h> //Used for the switch activation of the solenoid
/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
//  Joystick stick; // only joystick  *Was used in previous design kept here if we need to revert back*
	DigitalInput Dunker_switch; //This will be the switch on the board used to activate the
	DoubleSolenoid Frisbee_push; //The solenoid used to push the Frisbee out of the hopper
	Timer counter; //Used to count time solenoid actication to I can close the solenoid after ~.5 secs
	DriverStation *ds; //Define driver station
	bool Dunker_Switch_ON; //Used to track is the switch is flipped
public:
	RobotDemo(void):
		myRobot(1, 2),	// these must be initialized in the same order
		stick(1),		// as they are declared above.
		Frisbee_push(2,3), //Ports the solenoid may not be on "2" and "3"
		Dunker_switch(5) //The inpput port may not be "5"
	{
		myRobot.SetExpiration(0.1);
		ds = DriverStation::GetInstance(); //Driver station stuff
	}
		

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			Dunker_Switch_ON = !ds->GetDigitalIn(5); //Set the dunker variable according the Digital IO of port 5
			if (Dunker_Switch_ON == 0)
			{
				//counter.Reset(); //Reset the counter *This is no longer needed, kept here in case we revert back*
				Frisbee_push.Set(0); //Open the solenoid
				//counter.Start(); //Start the counter *This is no longer needed, kept here in case we revert back*
			}
			//if (counter.HasPeriodPassed(1.00)) //Checks to see if 1 seconds has passed since the timer has started *This is no longer needed, kept here in case we revert back*
			else
			{
				Frisbee_push.Set(1); //Close the solenoid
				//counter.Stop(); //Stop the counter *This is no longer needed, kept here in case we revert back*
				//counter.Reset(); //Reset the counter *This is no longer needed, kept here in case we revert back*
			}	
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

